#include "Draw.h"
#include <SPI.h>

void setup(){ 
  Serial.begin(9600);

  pinMode(LATCH_DS, OUTPUT);
  pinMode(LATCH_NC, INPUT);
  pinMode(LATCH_SHCP, OUTPUT);
  pinMode(LATCH_STCP, OUTPUT); 

  digitalWrite(LATCH_SHCP, LOW);
  digitalWrite(LATCH_STCP, LOW);

  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV2);
}

void sphere(int r,Cube &cube,int cx,int cy,int cz,boolean invert=true) {
     for (int c=0;c<20;c++) {
    for (int z=0;z<cube.SIDESIZE;z++) {
      for (int x=0;x<cube.SIDESIZE;x++) {
        for (int y=0;y<cube.SIDESIZE;y++) {
          int hside=cube.SIDESIZE/2;
          int d=(x-cx)*(x-cx)+(y-cy)*(y-cy)+(z-cz)*(z-cz);
       //   Serial.println(d);
          if ( d < r*r) 
            cube.setPixel(x,y,z,true ^ invert);
          else 
            cube.setPixel(x,y,z,false ^ invert);
        }
      }
     cube.update(z);
    // delay(150);
    }
   }
}


const int radius=5;
void pulsingSphere(Cube &cube) {
  for (int p=0;p<cube.SIDESIZE;p++) { 
    for (int r=0;r<radius;r++) {
      sphere(r,cube,p,p,p);
    }
    for (int r=0;r<radius;r++) {
      sphere(r,cube,p,p,p,false);
    }
  }
  for (int p=cube.SIDESIZE-1;p>=0;p--) { 
    for (int r=0;r<radius;r++) {
      sphere(r,cube,p,p,p);
    }
    for (int r=0;r<radius;r++) {
      sphere(r,cube,p,p,p,false);
    }
  }
}

Cube cube;
float x=4.0,y=4.0,z=5.0;
float dx=1.0,dy=1.0,dz=1.0;

void loop() {
  pulsingSphere(cube);
}

void loopOld() {
  /*
  for(int layer = 0; layer < cube.SIDESIZE;  layer++){
    for(int x = 0; x < cube.SIDESIZE; x++){
      for(int y = 0; y < cube.SIDESIZE; y++)
        cube.setPixel(layer, x, y, true);
    }
    cube.update(layer);
    delay(150);
    cube.reset();
  }
  */ 
  
  
 // Draw::drawPlaneX(0, cube);
 // Draw::drawPlaneZ(7, cube);
  cube.setPixel((int)x,(int)y,(int)z,false);
  dz-=0.01;
  if (dz==0.0) dz=1.0;
  x+=dx;
  y+=dy;
  z+=dz;
  if (x<0 || x>=cube.SIDESIZE) dx*=-1;
  if (y<0 || y>=cube.SIDESIZE) dy*=-1;
  if (z<0 || z>=cube.SIDESIZE) dz*=-1;
//  cube.reset();
  cube.setPixel((int)x,(int)y,(int)z,true);
  cube.update((int)z);
  delay(100);
}

